#include "Collision_lyvm.h"
#include "xVMRelationStruct.h"
#include "DIVA_Manager.h"
//#include "cpvm/Tree_Node_cpvm.h"
#include "DragVMToDestination.h"

#include <cassert>

using namespace std ;
Collision_lyvm* Collision_lyvm::CollisionLVM=NULL;

/// constructor
Collision_lyvm::Collision_lyvm(void){
	turnon = false;
}

/// destructor
Collision_lyvm::~Collision_lyvm(void){
}

/// singleton method 
Collision_lyvm* Collision_lyvm::getInstance(){
	if (CollisionLVM == NULL)
		CollisionLVM = new Collision_lyvm();

	return CollisionLVM;
}
/*
xx collisionDirect() {
	vmRelationStruct _root = starting point
    traverse_all the VMs from the starting point
	to compute the diections
}
*/
std::vector<DIVA_VM*> Collision_lyvm::collision_handling(std::vector<DIVA_VM*> cant_move,DIVA_VM* vm){
	if(!DragVMToDestination::getInstance()->move_finish(vm)){ return cant_move;}
	srand( (unsigned)time( NULL ) );
	_allSameTypeVMs = xVMRelationStruct::getInstance()->VMRS_getSameTypeVMs(vm);
	std::vector<DIVA_VM*> collided_VMs = collidedVMs(vm);
	std::vector<DIVA_VM*>::iterator cant_move_itr;
	std::vector<DIVA_VM*>::iterator collided_itr;
	bool collision_cant_move = false;
	if(collided_VMs.size() == 0){
		return cant_move;
	}
	for(collided_itr = collided_VMs.begin();collided_itr != collided_VMs.end();collided_itr++){
		///取得可以移動的方向
		
		std::vector<Ogre::Vector3> collided_dirs = getRelatedDirections(*collided_itr,vm);
		DIVA_VM* collided_vm;
		///與不能被被移動的點發生碰撞了
		for(cant_move_itr = cant_move.begin(); cant_move_itr != cant_move.end(); cant_move_itr++){
			if((*cant_move_itr) == (*collided_itr) && DragVMToDestination::getInstance()->move_finish((vm))){
				collision_cant_move = true;
				collided_vm = *cant_move_itr;
			}
		}
		if(collision_cant_move){
			collision_cant_move = false;
		///---論文上所用的
		//	position(collided_dirs,vm);

	    ///----oopsla
			Ogre::Vector3 direction = vm->getPosition() - collided_vm->getPosition();
			direction.normalise();
			Ogre::Vector3 size = collided_vm->getActualSize();
			Ogre::Vector3 incr = (direction * size * 2) * Ogre::Math::Cos(Ogre::Math::TWO_PI / 6) ;
			Ogre::Vector3 newpos = vm->getPosition()+incr ;
		///----oopsla

//			move_to_destination(vm,newpos);
			continue;
		}
	///---論文上所用的	
	//	position(collided_dirs,*collided_itr);

    ///----oopsla
		Ogre::Vector3 dir =  (*collided_itr)->getPosition() - vm->getPosition();
		dir.normalise();
		Ogre::Vector3 size = (*collided_itr)->getActualSize();
		Ogre::Vector3 incr = (dir * size * 3) * Ogre::Math::Cos(Ogre::Math::TWO_PI / 6) ;
		Ogre::Vector3 newpos = (*collided_itr)->getPosition()+incr ;
	///---- oopsla

		move_to_destination((*collided_itr),newpos);
		
		cant_move.push_back((*collided_itr));
		///recursive
		cant_move = collision_handling(cant_move,(*collided_itr));
	}
	return cant_move;
}

void Collision_lyvm::move_to_destination(DIVA_VM *vm, Ogre::Vector3 end){
	DragVMToDestination::getInstance()->addNeedMoveVM(vm,end);
}
///算densityFunction這邊可以參考論文第37頁
std::multimap<Ogre::Vector3,int> Collision_lyvm::densityFunction(DIVA_VM *vm){
	
	assert(vm->getVMType() == "composite");
	std::vector<DIVA_VM*> _allSameTypeVMs = xVMRelationStruct::getInstance()->VMRS_getSameTypeVMs(vm);
	std::multimap<Ogre::Vector3,int> m;
	std::vector<DIVA_VM*>::iterator itr;
	
	Ogre::Vector3 vm_pos = vm->getPosition();
	Ogre::Vector3 vm_size = vm->getActualSize();
	/*   direction 分成 8 個區塊分別是
	     
		 X Y Z
 	     + + +   Px_Py_Pz
		 + + -   Px_Py_Nz
		 + - +   Px_Ny_Pz
		 + - -   Px_Ny_Nz
		 - + +   Nx_Py_Pz
		 - + -   Nx_Py_Nz
		 - - +   Nx_Ny_Pz
		 - - -	 Nx_Ny_Nz

		 這邊的 + - 都是與 vm 的相對位置
		 Px : 表示 x 座標比 vm 大
		 Nx : 表示 x 座標比 vm 小
	*/
	m.insert(std::make_pair(Ogre::Vector3(1,1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(1,1,-1),0));
	m.insert(std::make_pair(Ogre::Vector3(1,-1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(1,-1,-1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,1,-1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,-1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,-1,-1),0));

	for(itr = _allSameTypeVMs.begin(); itr != _allSameTypeVMs.end(); itr++){
		
		Ogre::Vector3 v_pos = (*itr)->getPosition();
		if((*itr)->getVMType() == vm->getVMType()){
			if((*itr) == vm){
				continue;
			}
			std::vector<Ogre::Vector3> dir = getRelatedDirections((*itr),vm);
			std::vector<Ogre::Vector3>::iterator itv;
			for(itv = dir.begin();itv != dir.end();itv++){
				std::multimap<Ogre::Vector3,int>::iterator itm;
				for(itm = m.begin();itm != m.end();itm++){
					if((*itm).first == (*itv)){
						int count = (*itm).second;
						count++;
						(*itm).second = count;
					}
				}
			}
		}
	}
	
	return m;

}

///取得被input 的 VM 所碰撞的VM
std::vector<DIVA_VM*> Collision_lyvm::collidedVMs(DIVA_VM *vm){
	std::vector<DIVA_VM *> ret ;
	std::vector<DIVA_VM *>::iterator itr ;
	Ogre::Vector3 vm_size = vm->getActualSize();
	
	std::vector<DIVA_VM*> _allSameTypeVMs = xVMRelationStruct::getInstance()->VMRS_getSameTypeVMs(vm);

	if(vm->getVMType() != "composite"){
		return ret;
	}

	for (itr= _allSameTypeVMs.begin(); itr != _allSameTypeVMs.end(); itr ++ ) {
		
		if((*itr) == vm){
			continue;
		}
		if ((*itr)->getVMType() == vm->getVMType()) {
			if(abs((*itr)->getPosition().x - vm->getPosition().x) < vm_size.x){
				if(abs((*itr)->getPosition().y - vm->getPosition().y) < vm_size.y){
					if(abs((*itr)->getPosition().z - vm->getPosition().z) < vm_size.z){
						ret.push_back(*itr);
					}
				}
			}	
		
		}
	}
	return ret ;
}
///算densityFunction這邊可以參考論文第38頁
std::multimap<Ogre::Vector3,int> Collision_lyvm::ballFieldFunction(DIVA_VM *vm,double level){
	assert(vm->getVMType() == "composite");
	std::vector<DIVA_VM*> _allSameTypeVMs = xVMRelationStruct::getInstance()->VMRS_getSameTypeVMs(vm);
	std::multimap<Ogre::Vector3,int> m;
	std::vector<DIVA_VM*>::iterator itr;
	
	Ogre::Vector3 vm_pos = vm->getPosition();
	
	double distance = getMoveDistance(vm);
	distance = distance*level;
	/*   direction 分成 8 個區塊分別是
	     
		 X Y Z
 	     + + +   Px_Py_Pz
		 + + -   Px_Py_Nz
		 + - +   Px_Ny_Pz
		 + - -   Px_Ny_Nz
		 - + +   Nx_Py_Pz
		 - + -   Nx_Py_Nz
		 - - +   Nx_Ny_Pz
		 - - -	 Nx_Ny_Nz

		 這邊的 + - 都是與 vm 的相對位置
		 Px : 表示 x 座標比 vm 大
		 Nx : 表示 x 座標比 vm 小
	*/

	m.insert(std::make_pair(Ogre::Vector3(1,1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(1,1,-1),0));
	m.insert(std::make_pair(Ogre::Vector3(1,-1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(1,-1,-1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,1,-1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,-1,1),0));
	m.insert(std::make_pair(Ogre::Vector3(-1,-1,-1),0));

	for(itr = _allSameTypeVMs.begin(); itr != _allSameTypeVMs.end(); itr++){
		
		Ogre::Vector3 v_pos = (*itr)->getPosition();
		if((*itr)->getVMType() == vm->getVMType()){
			if((*itr) == vm){
				continue;
			}
			std::vector<Ogre::Vector3> dir = getRelatedDirections((*itr),vm);
			std::vector<Ogre::Vector3>::iterator itv;
			for(itv = dir.begin();itv != dir.end();itv++){
				std::multimap<Ogre::Vector3,int>::iterator itm;
				for(itm = m.begin();itm != m.end();itm++){
					if((*itm).first == (*itv)){
						Ogre::Vector3 temp = v_pos-vm->getPosition();
						double d = Ogre::Math::Sqrt(temp.x*temp.x+temp.y*temp.y+temp.z*temp.z);
						if(d<distance){
							int count = (*itm).second;
							count++;
							(*itm).second = count;
						}
						
					}
				}
			}
		}
	}

	return m;
}
///這邊是計算被碰撞的VM是我的哪幾個方向，用來幫助我們在移動的時盡量不要往這幾個方向做移動
std::vector<Ogre::Vector3> Collision_lyvm::getRelatedDirections(DIVA_VM *vm_aim, DIVA_VM *vm_self){
	std::vector<Ogre::Vector3> ret;
	Ogre::Vector3 vm_aim_pos = vm_aim->getPosition();
	Ogre::Vector3 vm_self_pos = vm_self->getPosition();
	Ogre::Vector3 vm_size = vm_self->getActualSize();

	if((vm_aim_pos.x > vm_self_pos.x-vm_size.x/2) && (vm_aim_pos.y > vm_self_pos.y-vm_size.y/2) && (vm_aim_pos.z > vm_self_pos.z-vm_size.z/2)){
		ret.push_back(Ogre::Vector3(1,1,1));
	}
	if((vm_aim_pos.x > vm_self_pos.x-vm_size.x/2) && (vm_aim_pos.y > vm_self_pos.y-vm_size.y/2) && (vm_aim_pos.z < vm_self_pos.z+vm_size.z/2)){
		ret.push_back(Ogre::Vector3(1,1,-1));
	}
	if((vm_aim_pos.x > vm_self_pos.x-vm_size.x/2) && (vm_aim_pos.y < vm_self_pos.y+vm_size.y/2) && (vm_aim_pos.z > vm_self_pos.z-vm_size.z/2)){
		ret.push_back(Ogre::Vector3(1,-1,1));
	}
	if((vm_aim_pos.x > vm_self_pos.x-vm_size.x/2) && (vm_aim_pos.y < vm_self_pos.y+vm_size.y/2) && (vm_aim_pos.z < vm_self_pos.z+vm_size.z/2)){
		ret.push_back(Ogre::Vector3(1,-1,-1));
	}
	if((vm_aim_pos.x < vm_self_pos.x+vm_size.x/2) && (vm_aim_pos.y > vm_self_pos.y-vm_size.y/2) && (vm_aim_pos.z > vm_self_pos.z-vm_size.z/2)){
		ret.push_back(Ogre::Vector3(-1,1,1));
	}
	if((vm_aim_pos.x < vm_self_pos.x+vm_size.x/2) && (vm_aim_pos.y > vm_self_pos.y-vm_size.y/2) && (vm_aim_pos.z < vm_self_pos.z+vm_size.z/2)){
		ret.push_back(Ogre::Vector3(-1,1,-1));
	}
	if((vm_aim_pos.x < vm_self_pos.x+vm_size.x/2) && (vm_aim_pos.y < vm_self_pos.y+vm_size.y/2) && (vm_aim_pos.z > vm_self_pos.z-vm_size.z/2)){
		ret.push_back(Ogre::Vector3(-1,-1,1));
	}
	if((vm_aim_pos.x < vm_self_pos.x+vm_size.x/2) && (vm_aim_pos.y <= vm_self_pos.y+vm_size.y/2) && (vm_aim_pos.z < vm_self_pos.z+vm_size.z/2)){
		ret.push_back(Ogre::Vector3(-1,-1,-1));
	}

	assert(ret.size() != 0);

	return ret;
}
///決定往哪個方向移動以及移動到哪個位置
void Collision_lyvm::position(std::vector<Ogre::Vector3> collided_dirs,DIVA_VM* collided_vm){
	///ballFieldCount 開始的level
	double level = 1;
	///是否找到可以移動的方向了
	bool find_dir = false;
	///collided_vm要被移動的方向
	Ogre::Vector3 destination_dir = Ogre::Vector3(0,0,0);

	std::multimap<Ogre::Vector3,int>::iterator dens_itr;
	std::multimap<Ogre::Vector3,int>::iterator fieldCount_itr;
	std::multimap<Ogre::Vector3,int>::iterator d_itr;
	
	while(!find_dir){
		std::multimap<Ogre::Vector3,int> density = densityFunction((collided_vm));
		std::multimap<Ogre::Vector3,int> fieldCount = ballFieldFunction(collided_vm,level);
		std::multimap<Ogre::Vector3,int> DandB;
	
		///將density 和 ballFieldCount 相加
		for(dens_itr = density.begin();dens_itr != density.end();dens_itr++ )
			for(fieldCount_itr = fieldCount.begin();fieldCount_itr != fieldCount.end();fieldCount_itr++){
				if((*dens_itr).first == (*fieldCount_itr).first){
					DandB.insert(std::make_pair((*dens_itr).first, (*dens_itr).second + (*fieldCount_itr).second));
				}
		}
        ///find max densuty
		int max_density = 0;
		for(dens_itr = density.begin();dens_itr != density.end();dens_itr++ ){
			if((*dens_itr).second >= max_density){
				max_density = (*dens_itr).second;
			}	
		}
		///判斷要往哪一個方向的DandB最小
		
		d_itr = DandB.begin();
		int smallest = (*d_itr).second;
		d_itr++;
		for(d_itr;d_itr != DandB.end();d_itr++){
			///先取得最小值
			if((*d_itr).second <= smallest){
				smallest = (*d_itr).second;
			}
		}

		///刪除已經有碰撞事實的方向
		std::vector<Ogre::Vector3>::iterator it;
		std::multimap<Ogre::Vector3,int> small_DandB;
		for(d_itr = DandB.begin();d_itr != DandB.end();d_itr++){
			bool check = false;
			for(it = collided_dirs.begin();it != collided_dirs.end();it++){
					if((*d_itr).first == (*it)){
					check = true;
				}
			}
			if(!check){
				small_DandB.insert(*d_itr);
			}
		}
		///判斷在dandb中的最小值是否有重複
		int count = -1;
		if(small_DandB.size() == 0){
			///表示被碰撞的collided_vm 和我們要處裡的vm 幾乎重疊
			destination_dir = Ogre::Vector3(1,0,0);
			Ogre::Vector3 size = collided_vm->getActualSize();
			Ogre::Vector3 incr = destination_dir * size * 2 ;
			Ogre::Vector3 newpos = collided_vm->getPosition()+incr ;
			//(*it)->setPosition(newpos);
			move_to_destination(collided_vm,newpos);
			return;
		}
		else if(small_DandB.size() == 1){
			find_dir = true;
		}
		else{
			for(d_itr = small_DandB.begin();d_itr != small_DandB.end();d_itr++){
				if((*d_itr).second == smallest){
					count++;
				}
			}
			if(count == 0 || smallest == 0 || level == max_density){
				find_dir = true;
			}
		}

		if(!find_dir){
			level++;
			continue;
		}
		else{
			d_itr = small_DandB.begin();
			int move_steps = get_number(small_DandB.size());
			for(int i = 0;i < move_steps;i++){
				d_itr++;
			}
			destination_dir = (*d_itr).first;
			assert(destination_dir != Ogre::Vector3(0,0,0));
			Ogre::Vector3 size = collided_vm->getActualSize();
			Ogre::Vector3 incr = destination_dir * size * 2 ;
			Ogre::Vector3 newpos = collided_vm->getPosition()+incr ;
			//(*it)->setPosition(newpos);
			move_to_destination(collided_vm,newpos);
		}

	}

}
double Collision_lyvm::getMoveDistance(DIVA_VM *vm){
	Ogre::Vector3 vm_size = vm->getActualSize();
	double distance = Ogre::Math::Sqrt(vm_size.x*vm_size.x+vm_size.y*vm_size.y+vm_size.z*vm_size.z);
	return distance;
}
int Collision_lyvm::get_number(int number){
	int ra = rand()%number-1;
	return ra;
}
///判斷目前Collision是否是open
bool Collision_lyvm::lyvmIsTurnOn(){
	return turnon;
}
///設定開關
void Collision_lyvm::switcher(bool b){
	this->turnon = b;
}